#include "hw_motor.h"

// 设置左电机速度
// 仅本文件使用
static void set_left_motor(int ain1, int ain2)
{
    DL_TimerG_setCaptureCompareValue(PWM_MOTORL_INST,ain1,GPIO_PWM_MOTORL_C0_IDX);
    DL_TimerG_setCaptureCompareValue(PWM_MOTORL_INST,ain2,GPIO_PWM_MOTORL_C1_IDX);
}
// 设置右电机速度
// 仅本文件使用
static void set_right_motor(int bin1, int bin2)
{
    DL_TimerA_setCaptureCompareValue(PWM_MOTORR_INST,bin1,GPIO_PWM_MOTORR_C0_IDX);
    DL_TimerA_setCaptureCompareValue(PWM_MOTORR_INST,bin2,GPIO_PWM_MOTORR_C1_IDX);    
}
// 限制输入的PWM最大最小值
// 仅本文件使用
static void restrict_pwm_max_value(int* value)
{
    if( *value > MOTOR_PWM_MAX ) 
    {
        *value = MOTOR_PWM_MAX;
    }
    if( *value < (-MOTOR_PWM_MAX) ) 
    {
        *value = -MOTOR_PWM_MAX;
    }
}

// 设置左右电机速度
// left_speed=左电机速度，范围-4999~4999
// right_speed=右电机速度，范围-4999~4999
// 备注： 负数反转 正数正转 0停止
void set_all_motor(int left_speed, int right_speed)
{
    restrict_pwm_max_value(&left_speed);
    restrict_pwm_max_value(&right_speed);

    if( left_speed > 0 )
    {
        set_left_motor(0, left_speed);
    }
    else if( left_speed < 0 )
    {
        left_speed = -left_speed;
        set_left_motor(left_speed, 0);
    }
    else 
    {
        set_left_motor(MOTOR_PWM_MAX,MOTOR_PWM_MAX);
    }

    if( right_speed > 0 )
    {
        set_right_motor(right_speed, 0);
    }
    else if( right_speed < 0 )
    {
        right_speed = -right_speed;
        set_right_motor(0, right_speed);
    }
    else {
        set_right_motor(MOTOR_PWM_MAX,MOTOR_PWM_MAX);
    }
}

// 左右电机刹车
void stop_motor(void)
{
    set_left_motor(MOTOR_PWM_MAX,MOTOR_PWM_MAX);
    set_right_motor(MOTOR_PWM_MAX,MOTOR_PWM_MAX);
}